#!/usr/bin/env python3
import rospy
from drone_pkg.msg import QRCode
import cv2
import numpy as np
from pyzbar.pyzbar import decode
import os
import time

def Laser_Init():
    os.system("gpio mode 10 out")
    os.system("gpio mode 13 out")
    os.system("gpio write 10 1")
    os.system("gpio write 13 1")

def Laser_Control(laser_id, state):
    if laser_id == 1 and state == 0:
        os.system("gpio write 10 1")
    if laser_id == 1 and state == 1:
        os.system("gpio write 10 0")
    if laser_id == 0 and state == 0:
        os.system("gpio write 13 1")
    if laser_id == 0 and state == 1:
        os.system("gpio write 13 0")

def Laser_ON(laser_id, t):
    Laser_Control(laser_id, 1)
    time.sleep(t * 0.1)
    Laser_Control(laser_id, 0)

def shutdown():
    Laser_Control(1, 0)
    Laser_Control(0, 0)
    os.system("gpio write 19 1")
    cv2.destroyAllWindows()  

def Read_Decode_Pic(name, image, CamID):
    global pub
    max_area = 0
    max_code = None

    # 检测二维码
    for code in decode(image):
        data = code.data.decode('utf-8')
        pts_poly = np.array(code.polygon, np.int32)  
        cv2.polylines(image, [pts_poly], True, (0, 255, 0), 5)  
        pts_rect = code.rect
        
        # 计算二维码的面积
        area = pts_rect[2] * pts_rect[3]
        
        # 找到面积最大的二维码
        if area > max_area:
            max_area = area
            max_code = {
                'data': data,
                'pts_rect': pts_rect,
                'cam_id': CamID
            }

    if max_code:
        # 获取最大的二维码的相关信息
        data = max_code['data']
        pts_rect = max_code['pts_rect']
        CamID = max_code['cam_id']
        rospy.loginfo("%d, %d", int(CamID), int(data))
        
        # 标记最大二维码
        cv2.putText(image, data, (pts_rect[0], pts_rect[1]), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
        center_x = pts_rect[0] + pts_rect[2] // 2
        center_y = pts_rect[1] + pts_rect[3] // 2
        cv2.circle(image, (center_x, center_y), 5, (0, 0, 255), -1)  # 红色圆点

        # 在二维码旁边写下中心坐标
        cv2.putText(image, f'Center: ({center_x}, {center_y})', (center_x + 10, center_y - 10), 
                    cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
        
        # 发送最大二维码的信息
        msg = QRCode()
        msg.Data = str(data)
        msg.CamID = CamID
        msg.x = pts_rect[0]
        msg.y = pts_rect[1]
        msg.Width = pts_rect[2]
        msg.Height = pts_rect[3]
        pub.publish(msg)

    # 转换图像到 HSV 色彩空间
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
    
    # 定义白色的HSV范围，白色通常具有高亮度和低饱和度
    lower_white = np.array([0, 0, 180])  # 低白色的范围，适度增加亮度下限
    upper_white = np.array([180, 40, 255])  # 高白色的范围，适度增加饱和度上限

    # 根据阈值生成白色掩模
    mask = cv2.inRange(hsv, lower_white, upper_white)
    
    # 显示掩模
    kernel = np.ones((5, 5), np.uint8)  # 5x5 的卷积核

    # 先腐蚀
    mask = cv2.erode(mask, kernel, iterations=2)

    # 再膨胀
    mask = cv2.dilate(mask, kernel, iterations=2)

    
    cv2.imshow("white_mask", mask)
    
    # 查找白色区域的轮廓
    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    for contour in contours:
        # 忽略小面积的区域
        if cv2.contourArea(contour) < 50000:
            continue
        
        # 获取矩形框
        x, y, w, h = cv2.boundingRect(contour)
        

        # 绘制长方形框并标记
        cv2.rectangle(image, (x, y), (x + w, y + h), (0, 255, 255), 3)
        
        # 发送白色色块作为二维码
        msg = QRCode()
        msg.Data = '999'  # 值为999的二维码
        msg.CamID = CamID
        msg.x = x
        msg.y = y
        msg.Width = w
        msg.Height = h
        pub.publish(msg)

    cv2.imshow(name, image)

if __name__ == '__main__':
    rospy.init_node("QRCode_detect", anonymous=True)
    rospy.on_shutdown(shutdown)
    os.system("gpio mode 19 out")
    os.system("gpio write 19 1")
    pub = rospy.Publisher("/QRCode_detect", QRCode, queue_size=10)
    rate = rospy.Rate(100)
    
    cap1 = cv2.VideoCapture('/dev/came1')
    
    count = 0
    flag = 0
    while not rospy.is_shutdown():
        ret, frame1 = cap1.read()
        count += 1
        try:   
            Read_Decode_Pic('1', frame1, 1) 
            if (count >= 10):
                count = 0
                flag = 1 - flag
                if (flag):
                    os.system("gpio write 19 0")
                else:
                    os.system("gpio write 19 1")

        except Exception as e:
            print(f"Error: {e}")

        cv2.waitKey(1)
        rate.sleep()

    cv2.destroyAllWindows()
